/**
 * File: protocol.h
 * Brief:
 * Created by: zhangping
 * Created at: 2017.06.5
 * Modified by:
 * Modified at:
 **/
 
#ifndef _MY_PROTOCOL_H_
#define _MY_PROTOCOL_H_


enum InsEnum{
  kVelMode=0,  // Work at Velocity model
  kPosMode=1   // Work at Position model
};


typedef unsigned char U8;

/** Institution protocol format*/
struct InsProtocol{
  U8 header1;
  U8 header2;
  U8 mode;
  U8 x[2];
  U8 y[2];
  U8 z[2];
  U8 orientation;
};

/** Instruction data*/
static const struct InsProtocol kInsProtocol[]={
  { 0xff,0xfe,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // Velocity mode
  { 0xff,0xfe,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // Position mode
};


/** Sensor data */
/** Ultrasonic data */
struct UltrasonicData{
  U8 range;	 // distance
  U8 angle;  //
};

/** Gyroscope data */
struct GyroscopeData{
  U8 h_x;  // High 8bit for x-axis.
  U8 l_x;  // Low 8bit for x-axis.
  U8 h_y;
  U8 l_y;
  U8 h_z;
  U8 l_z;
  U8 h_av; // High 8bit for angular velocity.
  U8 l_av; // Low 8bit for angular velocity.
};

/** Geomagnetic sensor data */
struct GeomagneticData{
  U8 roll_l;
  U8 roll_h;
  U8 pitch_l;
  U8 pitch_h;
  U8 yaw_l;
  U8 yaw_h;
  U8 t_l;
  U8 t_h;
  U8 sum;
};

/** Position data */
struct PositionData{
  U8 sensor_id;
  U8 x;
  U8 y;
  U8 z;
};

/** Sensor data */
union SensorData{
  UltrasonicData ult;
  GyroscopeData gyro;
  GeomagneticData geomag;
  PositionData pos;
};

struct SensorFrameData{
    U8 header0;
    U8 header1;
    U8 header2;
    U8 type;
    SensorData uData;
    U8 end;	
};


/** Sensor initial data */
const U8 kFrameHeader0 = 0xff;
const U8 kFrameHeader1 = 0xfe;
const U8 kFrameHeader2 = 0xfd;
const U8 kFrameEnd = 0xfc;

const U8 kTypeUltr = 0x01;
const U8 kTypeGyro = 0x02;
const U8 kTypeGeomag = 0x03;
const U8 kTypePos = 0x04;

static const SensorFrameData kSFData[] = { 
	{0xff, 0xfe, 0xfd, kTypeUltr,   {0}, 0xfc}, // UltrData
	{0xff, 0xfe, 0xfd, kTypeGyro,   {0}, 0xfc}, // Gyroscope
	{0xff, 0xfe, 0xfd, kTypeGeomag, {0}, 0xfc}, // Geomaginic 
	{0xff, 0xfe, 0xfd, kTypePos,    {0}, 0xfc}  // Localization
};


#endif
